
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mavproxy.c
  * @author     baiyang
  * @date       2021-7-22
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mavproxy.h"

#include <string.h>

#include <common/grapilot.h>
#include <common/console/console.h>

#include <ftp/ftp_manager.h>
#include "mavproxy_monitor.h"
#include "mavcmd.h"
#include "mavlink_param.h"
#include "mavlink_status.h"
#include "mavproxy_dev.h"

#include <rtthread.h>
#include <stdbool.h>

#include <fms.h>
#include <common/time/gp_time.h>
#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <common/gp_math/gp_mathlib.h>

#include <board_config/borad_config.h>

#include <sensor_compass/sensor_compass.h>
/*-----------------------------------macro------------------------------------*/
#define OS_ENTER_CRITICAL        rt_enter_critical()
#define OS_EXIT_CRITICAL         rt_exit_critical()

#define GP_MAVLINK_SYS_ID  1
#define GP_MAVLINK_COMP_ID 1
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static char thread_comm_stack[1024*10];
struct rt_thread thread_comm_handle;

static uint8_t mav_tx_buff[MAVLINK_MAX_PACKET_LEN];
static mavlink_system_t mavlink_system;
static rt_mutex_t _mavproxy_tx_lock;

static struct rt_timer timer_mavproxy;
static struct rt_event event_mavproxy;

static MAV_PeriodMsg_Queue _period_msg_queue;
static MAV_ImmediateMsg_Queue _imm_msg_queue;

static rt_device_t _mav_console_dev;
uint8_t _mav_dev_chan = 1; /* mavproxy device channel */

static int usb_is_connected = 0;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
extern rt_err_t mavlink_console_init(void);
extern void mav_console_handle_timeout(void);

void usbd_is_connected(int connect)
{
    /* channel 0: usart    channel 1: usb */
    if (usb_is_connected && _mav_dev_chan != 1) {
        _mav_dev_chan = 1;
    }

    if (!usb_is_connected && _mav_dev_chan != 0) {
        _mav_dev_chan = 0;
    }
}

mavlink_system_t mavproxy_get_system(void) 
{
    return mavlink_system;
}

static void timer_mavproxy_update(void* parameter)
{
    rt_event_send(&event_mavproxy, EVENT_MAVPROXY_UPDATE);
}

static void mavproxy_msg_heartbeat_pack(mavlink_message_t* msg_t)
{
    mavlink_heartbeat_t heartbeat;
    // uint16_t len;

    heartbeat.type = mavproxy_get_frame_mav_type();
    heartbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA;
    // TODO, fill base_mode and custom_mode
    heartbeat.base_mode = mavproxy_base_mode();
    heartbeat.custom_mode = mavproxy_custom_mode();
    heartbeat.system_status = mavproxy_vehicle_system_status();

    mavlink_msg_heartbeat_encode(mavlink_system.sysid, mavlink_system.compid,
        msg_t, &heartbeat);
}

static void mavproxy_msg_sys_status_pack(mavlink_message_t* msg_t)
{
    mavlink_sys_status_t sys_status;
    // uint16_t len;

    sys_status.onboard_control_sensors_present = 1;
    sys_status.onboard_control_sensors_enabled = 1;
    sys_status.onboard_control_sensors_health = 1;
    sys_status.load = 0;                            // CPU利用率
    sys_status.voltage_battery = 11000;
    sys_status.current_battery = -1;
    sys_status.battery_remaining = -1;

    mavlink_msg_sys_status_encode(mavlink_system.sysid, mavlink_system.compid,
        msg_t, &sys_status);
}

// TODO:还有数据需要填充,20210721
static void mavproxy_msg_vfr_hud_pack(mavlink_message_t *msg_t)
{
    mavlink_vfr_hud_t vfr_hud = {0};

    uitc_vehicle_position pos_info = {0};
    itc_copy_from_hub(ITC_ID(vehicle_position), &pos_info);
    
    uitc_vehicle_alt alt_info = {0};
    itc_copy_from_hub(ITC_ID(vehicle_alt), &alt_info);

    uitc_vehicle_attitude vehicle_attitude = {0};
    itc_copy_from_hub(ITC_ID(vehicle_attitude), &vehicle_attitude);

    if (fms.motors != NULL) {
        vfr_hud.throttle = Motors_get_throttle(fms.motors) * 100.0f;
    }

    vfr_hud.airspeed = 0;
    vfr_hud.groundspeed = sqrtf(pos_info.vx*pos_info.vx+pos_info.vy*pos_info.vy);
    vfr_hud.alt      = alt_info.relative_alt;
    vfr_hud.climb    = alt_info.vz;
    vfr_hud.heading = math_wrap_360(vehicle_attitude.vehicle_euler.yaw*RAD_TO_DEG);

    mavlink_msg_vfr_hud_encode(mavlink_system.sysid, mavlink_system.compid,msg_t, &vfr_hud);
}

void mavproxy_msg_gps_raw_int_pack(mavlink_message_t *msg_t)
{
    mavlink_gps_raw_int_t gps_raw_int = {0};
    uint16_t len;

    uitc_sensor_gps gps_pos_t = {0};
    int res = itc_copy_from_hub(ITC_ID(sensor_gps), &gps_pos_t);
    
    if(res == 0){
        gps_raw_int.lat = gps_pos_t.lat;
        gps_raw_int.lon = gps_pos_t.lon;
        gps_raw_int.alt = gps_pos_t.alt;
        gps_raw_int.eph = gps_pos_t.horizontal_accuracy*1e2;
        gps_raw_int.epv = gps_pos_t.vertical_accuracy*1e2;
        gps_raw_int.vel = gps_pos_t.vel_m_s*1e2;
        gps_raw_int.cog = Rad2Deg(gps_pos_t.cog_rad)*1e2;
        gps_raw_int.fix_type = gps_pos_t.fix_type;
        gps_raw_int.satellites_visible = gps_pos_t.num_sats;

        mavlink_msg_gps_raw_int_encode(mavlink_system.sysid, mavlink_system.compid, msg_t, &gps_raw_int);
    }
}

void mavproxy_msg_scaled_imu_pack(mavlink_message_t *msg_t)
{
    mavlink_scaled_imu_t scaled_imu = {0};

    uitc_sensor_acc sensor_acc = {0};
    uitc_sensor_gyr sensor_gyr = {0};
    uitc_sensor_mag sensor_mag = {0};

    itc_copy_from_hub(ITC_ID(sensor_acc), &sensor_acc);
    itc_copy_from_hub(ITC_ID(sensor_gyr), &sensor_gyr);
    itc_copy_from_hub(ITC_ID(sensor_mag), &sensor_mag);
    
    scaled_imu.xacc = (int16_t)(sensor_acc.sensor_acc_filter[0]*1000.0f) / GRAVITY_MSS;
    scaled_imu.yacc = (int16_t)(sensor_acc.sensor_acc_filter[1]*1000.0f) / GRAVITY_MSS;
    scaled_imu.zacc = (int16_t)(sensor_acc.sensor_acc_filter[2]*1000.0f) / GRAVITY_MSS;
    scaled_imu.xgyro = (int16_t)(sensor_gyr.sensor_gyr_filter[0]*1000.0f);
    scaled_imu.ygyro = (int16_t)(sensor_gyr.sensor_gyr_filter[1]*1000.0f);
    scaled_imu.zgyro = (int16_t)(sensor_gyr.sensor_gyr_filter[2]*1000.0f);
    scaled_imu.xmag = (int16_t)(sensor_mag.sensor_mag_correct[0]*1000.0f);
    scaled_imu.ymag = (int16_t)(sensor_mag.sensor_mag_correct[1]*1000.0f);
    scaled_imu.zmag = (int16_t)(sensor_mag.sensor_mag_correct[2]*1000.0f);
    
    mavlink_msg_scaled_imu_encode(mavlink_system.sysid, mavlink_system.compid, msg_t, &scaled_imu);
}

void mavproxy_msg_attitude_pack(mavlink_message_t *msg_t)
{
    mavlink_attitude_t attitude = {0};

    uitc_vehicle_attitude vehicle_attitude = {0};
    uitc_sensor_gyr sensor_gyr = {0};

    itc_copy_from_hub(ITC_ID(vehicle_attitude), &vehicle_attitude);
    itc_copy_from_hub(ITC_ID(sensor_gyr), &sensor_gyr);
    
    attitude.roll = vehicle_attitude.vehicle_euler.roll;
    attitude.pitch = vehicle_attitude.vehicle_euler.pitch;
    attitude.yaw = vehicle_attitude.vehicle_euler.yaw;
    attitude.rollspeed = sensor_gyr.sensor_gyr_filter[0];
    attitude.pitchspeed = sensor_gyr.sensor_gyr_filter[1];
    attitude.yawspeed = sensor_gyr.sensor_gyr_filter[2];
    
    mavlink_msg_attitude_encode(mavlink_system.sysid, mavlink_system.compid, msg_t, &attitude);
}

void mavproxy_msg_altitude_pack(mavlink_message_t *msg_t)
{
    mavlink_altitude_t altitude = {0};
    uitc_vehicle_alt vehicle_alt = {0};
    
    itc_copy_from_hub(ITC_ID(vehicle_alt), &vehicle_alt);
    
    altitude.altitude_monotonic = 0;
    altitude.altitude_amsl = vehicle_alt.alt;
    altitude.altitude_local = 0;
    altitude.altitude_relative = vehicle_alt.relative_alt;
    altitude.altitude_terrain = 0;
    
    mavlink_msg_altitude_encode(mavlink_system.sysid, mavlink_system.compid, msg_t, &altitude);
}

void mavproxy_msg_scaled_pressure_pack(mavlink_message_t *msg_t)
{
    mavlink_scaled_pressure_t scaled_pressure = {0};
    uitc_sensor_baro_raw report;
    if (itc_copy_from_hub(ITC_ID(sensor_baro_raw), &report) == 0 && report.baro_raw[0].timestamp_us != 0)
    {
        scaled_pressure.time_boot_ms = report.baro_raw[0].timestamp_us*0.001f;
        scaled_pressure.press_abs = report.baro_raw[0].pressure_pa * 0.01f;
        scaled_pressure.temperature = report.baro_raw[0].temperature_deg * 100.0f;
        scaled_pressure.press_diff = (report.baro_raw[0].pressure_pa - report.baro_raw[0].ground_pressure_pa) * 0.01f;

        mavlink_msg_scaled_pressure_encode(mavlink_system.sysid, mavlink_system.compid,msg_t, &scaled_pressure);
    }
}

void mavproxy_msg_scaled_pressure2_pack(mavlink_message_t *msg_t)
{
    mavlink_scaled_pressure2_t scaled_pressure = {0};
    uitc_sensor_baro_raw report;
    if (itc_copy_from_hub(ITC_ID(sensor_baro_raw), &report) == 0 && report.baro_raw[1].timestamp_us != 0)
    {
        scaled_pressure.time_boot_ms = report.baro_raw[1].timestamp_us*0.001f;
        scaled_pressure.press_abs = report.baro_raw[1].pressure_pa * 0.01f;
        scaled_pressure.temperature = report.baro_raw[1].temperature_deg * 100.0f;
        scaled_pressure.press_diff = (report.baro_raw[1].pressure_pa - report.baro_raw[0].ground_pressure_pa) * 0.01f;

        mavlink_msg_scaled_pressure2_encode(mavlink_system.sysid, mavlink_system.compid,msg_t, &scaled_pressure);
    }
}

void mavproxy_msg_scaled_pressure3_pack(mavlink_message_t *msg_t)
{
    mavlink_scaled_pressure3_t scaled_pressure = {0};
    uitc_sensor_baro_raw report;
    if (itc_copy_from_hub(ITC_ID(sensor_baro_raw), &report) == 0 && report.baro_raw[2].timestamp_us != 0)
    {
        scaled_pressure.time_boot_ms = report.baro_raw[2].timestamp_us*0.001f;
        scaled_pressure.press_abs = report.baro_raw[2].pressure_pa * 0.01f;
        scaled_pressure.temperature = report.baro_raw[2].temperature_deg * 100.0f;
        scaled_pressure.press_diff = (report.baro_raw[2].pressure_pa - report.baro_raw[0].ground_pressure_pa) * 0.01f;

        mavlink_msg_scaled_pressure3_encode(mavlink_system.sysid, mavlink_system.compid,msg_t, &scaled_pressure);
    }
}

#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
void mavproxy_msg_mag_cal_progress_pack(mavlink_message_t *msg_t)
{
    sensor_compass_send_mag_cal_progress(msg_t);
}

void mavproxy_msg_mag_cal_report_pack(mavlink_message_t *msg_t)
{
    sensor_compass_send_mag_cal_report(msg_t);
}
#endif

void mavproxy_msg_rc_channels_raw_pack(mavlink_message_t *msg_t)
{
    uitc_actuator_rc actuator_rc = {0};
    mavlink_rc_channels_raw_t rc_raw = {0};

    itc_copy_from_hub(ITC_ID(vehicle_rc), &actuator_rc);

    if (actuator_rc.channel_count > 8) {
        return;
    }

    rc_raw.time_boot_ms = actuator_rc.timestamp_us*0.001;

    rc_raw.chan1_raw = actuator_rc.channels[0];
    rc_raw.chan2_raw = actuator_rc.channels[1];
    rc_raw.chan3_raw = actuator_rc.channels[2];
    rc_raw.chan4_raw = actuator_rc.channels[3];
    rc_raw.chan5_raw = actuator_rc.channels[4];
    rc_raw.chan6_raw = actuator_rc.channels[5];
    rc_raw.chan7_raw = actuator_rc.channels[6];
    rc_raw.chan8_raw = actuator_rc.channels[7];
    rc_raw.rssi      = actuator_rc.rssi;
    
    mavlink_msg_rc_channels_raw_encode(mavlink_system.sysid, mavlink_system.compid,msg_t, &rc_raw);
}

/*
  send RC_CHANNELS messages
 */
void mavproxy_msg_send_rc_channels_pack(mavlink_message_t *msg_t)
{
    uitc_actuator_rc actuator_rc = {0};
    mavlink_rc_channels_t rc_raw = {0};

    itc_copy_from_hub(ITC_ID(vehicle_rc), &actuator_rc);

    if (actuator_rc.channel_count <= 8) {
        return;
    }

    rc_raw.time_boot_ms = actuator_rc.timestamp_us*0.001;

    rc_raw.chan1_raw = actuator_rc.channels[0];
    rc_raw.chan2_raw = actuator_rc.channels[1];
    rc_raw.chan3_raw = actuator_rc.channels[2];
    rc_raw.chan4_raw = actuator_rc.channels[3];
    rc_raw.chan5_raw = actuator_rc.channels[4];
    rc_raw.chan6_raw = actuator_rc.channels[5];
    rc_raw.chan7_raw = actuator_rc.channels[6];
    rc_raw.chan8_raw = actuator_rc.channels[7];
    rc_raw.chan9_raw = actuator_rc.channels[8];
    rc_raw.chan10_raw = actuator_rc.channels[9];
    rc_raw.chan11_raw = actuator_rc.channels[10];
    rc_raw.chan12_raw = actuator_rc.channels[11];
    rc_raw.chan13_raw = actuator_rc.channels[12];
    rc_raw.chan14_raw = actuator_rc.channels[13];
    rc_raw.chan15_raw = actuator_rc.channels[14];
    rc_raw.chan16_raw = actuator_rc.channels[15];
    rc_raw.chan17_raw = actuator_rc.channels[16];
    rc_raw.chan18_raw = actuator_rc.channels[17];

    rc_raw.chancount  = actuator_rc.channel_count;
    rc_raw.rssi       = actuator_rc.rssi;

    mavlink_msg_rc_channels_encode(mavlink_system.sysid, mavlink_system.compid,msg_t, &rc_raw);
}

void mavproxy_msg_servo_output_pack(mavlink_message_t *msg_t)
{
    uitc_actuator_outputs actuator_outputs = {0};
    mavlink_servo_output_raw_t srv_out ={0};
    
    itc_copy_from_hub(ITC_ID(vehicle_actuator_outputs), &actuator_outputs);
    srv_out.time_usec = actuator_outputs.timestamp_us;
    srv_out.servo1_raw = actuator_outputs.output[0];
    srv_out.servo2_raw = actuator_outputs.output[1];
    srv_out.servo3_raw = actuator_outputs.output[2];
    srv_out.servo4_raw = actuator_outputs.output[3];
    srv_out.servo5_raw = actuator_outputs.output[4];
    srv_out.servo6_raw = actuator_outputs.output[5];
    srv_out.servo7_raw = actuator_outputs.output[6];
    srv_out.servo8_raw = actuator_outputs.output[7];
    srv_out.servo9_raw = actuator_outputs.output[8];
    srv_out.servo10_raw = actuator_outputs.output[9];
    srv_out.servo11_raw = actuator_outputs.output[10];
    srv_out.servo12_raw = actuator_outputs.output[11];
    
    mavlink_msg_servo_output_raw_encode(mavlink_system.sysid, mavlink_system.compid,msg_t, &srv_out);
}

void mavproxy_send_statustext(uint8_t severity, const char *fmt, ...)
{
    mavlink_message_t msg;
    mavlink_statustext_t statustext = {0};
    memset(statustext.text, 0, sizeof(statustext.text));

    va_list args;
    int length;
    
    va_start(args, fmt);
    length = vsnprintf(statustext.text, sizeof(statustext.text), fmt, args);
    va_end(args);
    
    statustext.severity = severity;

    mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext);
    mavproxy_send_immediate_msg(&msg, 1);
}

void mavproxy_send_textv(uint8_t severity, const char *fmt, va_list arg_list)
{
    mavlink_message_t msg;
    mavlink_statustext_t statustext = {0};
    memset(statustext.text, 0, sizeof(statustext.text));

    int length;
    
    length = vsnprintf(statustext.text, sizeof(statustext.text), fmt, arg_list);
    statustext.severity = severity;

    mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext);
    mavproxy_send_immediate_msg(&msg, 1);
}

static uint8_t try_send_immediate_msg(void)
{
    while (_imm_msg_queue.head != _imm_msg_queue.tail) {
        if (mavproxy_send_immediate_msg(&_imm_msg_queue.queue[_imm_msg_queue.tail], 1)) {
            _imm_msg_queue.tail = (_imm_msg_queue.tail + 1) % MAX_IMMEDIATE_MSG_QUEUE_SIZE;
        }
    }

    return 1;
}

static uint8_t try_send_period_msg(void)
{
    static mavlink_message_t msg = {0};

    for (uint16_t i = 0; i < _period_msg_queue.size; i++) {
        uint32_t now = time_millis();
        MAV_PeriodMsg* msg_t = &_period_msg_queue.queue[_period_msg_queue.index];
        _period_msg_queue.index = (_period_msg_queue.index + 1) % _period_msg_queue.size;

        // find next msg to be sent
        if (now - msg_t->time_stamp >= msg_t->period && msg_t->enable) {
            msg_t->time_stamp = now;
            msg_t->msg_pack_cb(&msg);

            if (msg_t->msgid != msg.msgid) {
                continue;
            }

            // send out msg
            mavproxy_send_immediate_msg(&msg, 1);
        }
    }

    return 0;
}

void mavproxy_send_event(uint32_t event_set)
{
    rt_event_send(&event_mavproxy, event_set);
}

uint8_t mavproxy_register_period_msg(
    uint32_t msgid, uint16_t period_ms,
    void (*msg_pack_cb)(mavlink_message_t* msg_t), uint8_t enable)
{
    MAV_PeriodMsg msg;

    msg.msgid = msgid;
    msg.enable = enable;
    msg.period = period_ms;
    msg.msg_pack_cb = msg_pack_cb;
    msg.time_stamp = 0;

    if (_period_msg_queue.size < MAX_PERIOD_MSG_QUEUE_SIZE) {
        _period_msg_queue.queue[_period_msg_queue.size++] = msg;

        return 1;
    } else {
        console_printf("mavproxy period msg queue is full\n");
        return 0;
    }
}

uint8_t mavproxy_send_immediate_msg(const mavlink_message_t* msg,
    uint8_t sync)
{
    /* if sync flag set, send out msg immediately */
    if (sync) {
        uint16_t len;
        rt_size_t size;

        /* make sure only one thread can access tx buffer at mean time. */
        rt_mutex_take(_mavproxy_tx_lock, RT_WAITING_FOREVER);

        len = mavlink_msg_to_send_buffer(mav_tx_buff, msg);
        size = mavproxy_dev_write(_mav_dev_chan, mav_tx_buff, len, RT_WAITING_FOREVER);

        rt_mutex_release(_mavproxy_tx_lock);

        return size == len ? 1 : 0;
    }

    /* otherwise, push msg into queue (asynchronize mode) */

    if ((_imm_msg_queue.head + 1) % MAX_IMMEDIATE_MSG_QUEUE_SIZE == _imm_msg_queue.tail) {
        return 0;
    }

    _imm_msg_queue.queue[_imm_msg_queue.head] = *msg;
    _imm_msg_queue.head = (_imm_msg_queue.head + 1) % MAX_IMMEDIATE_MSG_QUEUE_SIZE;

    /* wakeup mavproxy to send out temporary msg immediately */
    rt_event_send(&event_mavproxy, EVENT_MAVPROXY_UPDATE);

    return 1;
}

void mavproxy_loop()
{
    if (!brd_is_vehicle_init_finish()) {
        if (!brd_is_vehicle_esc_calibrate()) {
            return;
        }
    }

    mavproxy_check_usbd_connected();

    mavproxy_rx_loop();

    // try to send out immediate msg first
    try_send_immediate_msg();

    // try to send out periodical msg
    try_send_period_msg();

    // process mavlink command
    mavcmd_process();
}

void mavproxy_init()
{
    /* init mavlink system info */
    mavlink_system.sysid = GP_MAVLINK_SYS_ID;
    mavlink_system.compid = GP_MAVLINK_COMP_ID;

    /* init message queue */
    _period_msg_queue.size = 0;
    _period_msg_queue.index = 0;
    _imm_msg_queue.head = 0;
    _imm_msg_queue.tail = 0;

    mavproxy_dev_init(_mav_dev_chan);
    mavlink_console_init();

    _mavproxy_tx_lock = rt_mutex_create("mav_tx_lock", RT_IPC_FLAG_FIFO);

    /* get mavlink console device */
    _mav_console_dev = rt_device_find("mav_console");
    /* create event */
    rt_event_init(&event_mavproxy, "mavproxy", RT_IPC_FLAG_FIFO);

    /* register timer event */
    rt_timer_init(&timer_mavproxy, "comm_update", timer_mavproxy_update, RT_NULL, rt_tick_from_millisecond(10),
        RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_HARD_TIMER);
    rt_timer_start(&timer_mavproxy);

    /* register periodical mavlink msg */
    mavproxy_register_period_msg(MAVLINK_MSG_ID_HEARTBEAT, 1000,
        mavproxy_msg_heartbeat_pack, 1);
    
    mavproxy_register_period_msg(MAVLINK_MSG_ID_SYS_STATUS, 1000,
        mavproxy_msg_sys_status_pack, 1);
    
    mavproxy_register_period_msg(MAVLINK_MSG_ID_VFR_HUD, 200,
        mavproxy_msg_vfr_hud_pack, 1);
    
    mavproxy_register_period_msg(MAVLINK_MSG_ID_GPS_RAW_INT, 200,
        mavproxy_msg_gps_raw_int_pack, 1);
    
    mavproxy_register_period_msg(MAVLINK_MSG_ID_SCALED_IMU, 200,
        mavproxy_msg_scaled_imu_pack, 1);
    
    mavproxy_register_period_msg(MAVLINK_MSG_ID_ATTITUDE, 200,
        mavproxy_msg_attitude_pack, 1);
    
    mavproxy_register_period_msg(MAVLINK_MSG_ID_ALTITUDE, 200,
        mavproxy_msg_altitude_pack, 1);
    
    mavproxy_register_period_msg(MAVLINK_MSG_ID_SCALED_PRESSURE, 200,
        mavproxy_msg_scaled_pressure_pack, 1);
    mavproxy_register_period_msg(MAVLINK_MSG_ID_SCALED_PRESSURE2, 200,
        mavproxy_msg_scaled_pressure2_pack, 1);
    mavproxy_register_period_msg(MAVLINK_MSG_ID_SCALED_PRESSURE3, 200,
        mavproxy_msg_scaled_pressure3_pack, 1);

#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    mavproxy_register_period_msg(MAVLINK_MSG_ID_MAG_CAL_PROGRESS, 300,
        mavproxy_msg_mag_cal_progress_pack, 1);
    mavproxy_register_period_msg(MAVLINK_MSG_ID_MAG_CAL_REPORT, 300,
        mavproxy_msg_mag_cal_report_pack, 1);
#endif

    mavproxy_register_period_msg(MAVLINK_MSG_ID_RC_CHANNELS_RAW, 200, mavproxy_msg_rc_channels_raw_pack,1);
    mavproxy_register_period_msg(MAVLINK_MSG_ID_RC_CHANNELS, 200, mavproxy_msg_send_rc_channels_pack,1);
    mavproxy_register_period_msg(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 250, mavproxy_msg_servo_output_pack,1);
}

gp_err task_comm_init(void)
{
    rt_err_t res;

    res = rt_thread_init(&thread_comm_handle,
        "mavproxy",
        task_comm_entry,
        RT_NULL,
        &thread_comm_stack[0],
        sizeof(thread_comm_stack), PRIORITY_COMM, 5);
    RT_ASSERT(res == RT_EOK);
    rt_thread_startup(&thread_comm_handle);

    brd_set_vehicle_init_stage(INIT_STAGE_COMM);

    return GP_EOK;
}

void task_comm_entry(void* parameter)
{
    rt_err_t res;
    rt_uint32_t recv_set = 0;
    rt_uint32_t wait_set = EVENT_MAVPROXY_UPDATE | EVENT_MAVCONSOLE_TIMEOUT | EVENT_SEND_ALL_PARAM;

    while (1) {
        /* wait event occur */
        res = rt_event_recv(&event_mavproxy, wait_set,
            RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR,
            RT_WAITING_FOREVER, &recv_set);

        if (res == RT_EOK) {
            if (recv_set & EVENT_SEND_ALL_PARAM) {
                mavlink_param_send_all();
            }

            if (recv_set & EVENT_MAVCONSOLE_TIMEOUT) {
                mav_console_handle_timeout();
            }

            if (recv_set & EVENT_MAVPROXY_UPDATE) {
                mavproxy_loop();
            }
        } else {
            // some err happen
            console_printf("mavproxy loop, err:%d\r\n", res);
        }
    }
    res = 0;
}

/*------------------------------------test------------------------------------*/


